/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : pwm.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
 
#include "pwm.h"
#include "pipo.h"

int16_t		pwm_in_val[NUMPPM_IN];
int16_t		pwm_in_delta[NUMPPM_IN];
int16_t		pwm_in_bias[4];
uint16_t	ch_out_val[NUMPPM_OUT]; //var to hold the outgoing values
uint8_t		ch_out_frameCount[NUMPPM_OUT];
uint16_t	pwm_out_ticks[NUMPPM_OUT+1];
uint8_t		pwm_out_bits[NUMPPM_OUT+1];
uint8_t		idx;

/*
 * INIT PWM 
 *
 */
void pwm_init() 
{
	for(uint8_t i=0; i<NUMPPM_OUT; i++)
    {
        pwm_out_set(i+1,-RESX); //set throttles to min
        pwm_in_val[i] = RESX;
		ch_out_frameCount[i] = 0x00; // used as a bit mask = (# of frames to repeat -1)
		/* i.e. if every frame (so every 1 frame count) use 1 -1 = 0x00
		 * for every other frame i.e. every 2 frames then 2-1 = 0x01
		 * every 16 frames = 0x0F etc
		 * ## must be a power of 2 ##
		 */
    }
}	

/*
 * Set data in pwm out buffer
 * parameter is -RESX...+RESX
 * stored as # of ticks to be added to min PW
 * stored range 0 - (2 * RESX)
 */
void pwm_out_set(uint8_t ch, int16_t v) 
{
	if(v<(-RESX))
        ch_out_val[ch-1] = 0;
    else if(v>RESX)
        ch_out_val[ch-1] = RESX*2;
    else
        ch_out_val[ch-1] = v + RESX;
}

/*
 * Zero stick inputs
 * 
 */
void calibrateSticks() {	
    int32_t accumulator[4] = {0};
    uint16_t counter = 0;

    while (counter<CALIB_SAMPLES*2)
    {
        LED_GREEN_FLIP;LED_RED_FLIP;  // flash green led to signify calibration
        uint16_t cg25ms = g25msec + 4; // current timer - make sure we don't read more than once every 100ms
        counter++;

		accumulator[0] += pwm_in_val[0];
		accumulator[1] += pwm_in_val[1];
		accumulator[3] += pwm_in_val[3];
		
		while (cg25ms != g25msec);  // wait for next 100ms period - one sample every 100ms
    }

    if(counter>0)
    {
        for(uint8_t i=0; i<=3; i++)
            pwm_in_bias[i] = int16_t(accumulator[i]/counter);
    }
}


/*
 * Get pwm in data
 * ch is 1 - NUMPPM_OUT
 * actual storage array is 0 based
 */
int16_t pwmInRaw(uint8_t ch) {
    return pwm_in_val[ch-1];// & 0xFFFC;
}

/*
 * Get pwm in delta
 * ch is 1 - NUMPPM_OUT
 * actual storage array is 0 based
 */
int16_t pwmInDelta(uint8_t ch) {
    return pwm_in_delta[ch-1];
}

int16_t pwmInBias(uint8_t ch) {
    return pwm_in_bias[ch-1];
}

/*
 * pwm in reader
 * interrupt fires whenever one of the PWM_IN pins change, NOBLOCK so pwm out can run uninterrupted
 */
ISR(PCINT0_vect)  // blocking so we need to enable ASAP
{
    static uint16_t dt,dtt;  //using static to prevent push/pop every time ISR is called
    static uint8_t  pwm_in_state, chng;
    static uint16_t pwm_in_timer[NUMPPM_IN];

    dtt=TCNT1; //read current timer to dt;
    
	sei();
    chng = pwm_in_state ^ PWM_IN_MASK;
    pwm_in_state = PWM_IN_MASK;

    for(uint8_t i=0; i<NUMPPM_IN; i++)
        if(chng & (1<<i)) 
		{
            dt = dtt-pwm_in_timer[i]; //dt now holds the delta of the current timing
            pwm_in_timer[i] = dtt; //pwm_in_timer is set to current time
            if ((dt>PWM_STICK_MIN) && (dt<PWM_STICK_MAX))  // allows for radio outputting upto 125%
			{  // 2000 ticks per 1msec.   (1.5*2000 +-1024) -> valid value  this way we don't jave to worry about polarity
				
				if (dt>PWM_MAX) dt=PWM_MAX;	if (dt<PWM_MIN) dt=PWM_MIN; // scale back to max +/- 100%
                pwm_in_delta[i] = -pwm_in_val[i];
				pwm_in_val[i]	=  ((dt-PWM_MIDFRAME) & 0xFFFC); // servo value = time-3000
				// ### TODO ### need to look at how accurate this routine is i.e. is can be interrupted
				// and truncate the val as appropriate so that stored val is steady rather than bouncing around
				pwm_in_delta[i] += pwm_in_val[i]; //pwm_in_delta = new pwm_in_val - last pwm_in_val
			}			
        }
}



ISR(PCINT1_vect, ISR_ALIASOF(PCINT0_vect));  // declare pcint1 same as pcint0


/*************************************************************************
 setup pwm out timers array and port values
 called synchronously from main loop when there is new data to send
 channels are started every ch_out_frameCount times
 channels are stopped every frame - but obviously this doesn't actually do anything
*************************************************************************/
void setupPWMs()
{	
	static uint8_t frameCount;
	uint8_t i, j, frameBits, index;
	
	
    cli();
	PORTD = pwm_out_bits[0];		//start frames (set up by last round)
	uint16_t frameStart = TCNT1;	// store time of frame start
	sei();
	frameCount++;
	frameBits = pwm_out_bits[0];	// start of frame bits - save for later
    
	
	// initialise pwn_out array
	// pwm_out_tick & pwm_out_bits index 0 is the frame start
	// from then on ticks holds the # of ticks to wait 
	//and bits holds the value to be put on the o/p
	pwm_out_ticks[0]=1; // make sure it is not clobbered by the insert routine ##TODO get rid of this line
	pwm_out_bits[0]=0x00;	//reset the start bits
	for (i=1;i<=NUMPPM_OUT;i++)	{
		if (!(frameCount & ch_out_frameCount[i-1])) // ch_out_frameCount is a mask
				pwm_out_bits[0] |= (1<<(i-1)); // add channel i to start pulses
		pwm_out_ticks[i]= 0x00; // clear out the arrays
	}
	
    //set-up output array
    for(i=1; i<=NUMPPM_OUT; i++)
    {
	   //insert value to array according to timer order
        uint16_t v =  ch_out_val[i-1] + PWM_MIN; //time value for channel, ch_out_val is 0 based
        v &= 0xFFF8; // removing LSBs so we don't get overruns on the loop
        // v now holds the length of the pulse in ticks
		//insert v into array, by order
        index = 1;
        while ((index<=NUMPPM_OUT) && (v > pwm_out_ticks[index]) && pwm_out_ticks[index])
            index++;

        if(index<=NUMPPM_OUT)// should ALWAYS be true
        {
            if(v==pwm_out_ticks[index]) // this time same as an existing time 
            {
                pwm_out_bits[index] &= ~(1 << (i-1));  // so, just turn off the additional channel
            }
            else  // new time
            {
                //insert values
                if(pwm_out_ticks[index]) // if there is already a time in this slot
                {
                    for(j=(i); j>index; j--)
                    {
                        pwm_out_ticks[j] = pwm_out_ticks[j-1];
                        pwm_out_bits[j] = pwm_out_bits[j-1];
                    }
                }

                pwm_out_ticks[index] = v;
                pwm_out_bits[index] = ~(1 << (i-1));
            }
        }
		else // we should never get here!! 
			flashRedLed = 5;
	}
 
    //set up first loop
    //index = 0;

    //need to add pwm_frame to timers
    //also concatenate the port values
    for(i=1; i<=NUMPPM_OUT; i++)
		{
			pwm_out_ticks[i] += frameStart;
			uint8_t p = pwm_out_bits[i];
			frameBits &= p;
			pwm_out_bits[i] = frameBits;
		}
	
		idx=1;
		OCR1A = pwm_out_ticks[idx];	//setup for first timer
		TIFR1 |= (1<<OCF0A);		//clear OCR1A int flag
		TIMSK1 |= (1<<OCIE1A);		// enable interrupt on TCNT1 == OCR1A		
}


/*************************************************************************
 pwm out generator
 44 machine cycles
 ~6 timer ticks
 therefore make sure the pwm_ticks are & 0xFFF8 
*************************************************************************/
ISR(TIMER1_COMPA_vect, ISR_BLOCK)  // fires on TCNT1 == OCR1B - pwm out generator
{
    PORTD = pwm_out_bits[idx++]; //set bits (set up by last round) 
	OCR1A = pwm_out_ticks[idx];
	sei();
	if (!(idx<=NUMPPM_OUT)) TIMSK1 &= ~(1<<OCIE1A);		// enable interrupt on TCNT1 == OCR1A
}

